﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest.SelfLocators.MBL
{
  class SimilarityFunction : Function
  {
    List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces;
    Experiment experiment;
    WorldState templateWorldState;
    bool highWeightForNewerMeasurements;
    int n;

    public SimilarityFunction(
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, 
      Experiment experiment, 
      bool useLastMeasurementOnly)
    {
      this.listOfOldPlaces = listOfOldPlaces;
      this.experiment = experiment;
      templateWorldState = (WorldState)System.Activator.CreateInstance(experiment.simulatedWorldState.GetType());
      highWeightForNewerMeasurements = Globals.selfLocatorProperties.TypeOfLandmarkWeighting == WeightingType.indexPowerN;
      n = Globals.selfLocatorProperties._IndexPowerN;
    }

    public override double Evaluate(double[] vector)
    {
      double sumOfDistances = 0;
      double weight = 1;
      double index = 0;
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement worldStateAndObservation in listOfOldPlaces)
      {
        if (worldStateAndObservation.measurement != null)
        {
          if (highWeightForNewerMeasurements)
          {
            double value = Math.Max(n - index, 0);
            index++;
            weight = Math.Pow(value, 2);
            if (index >= n) break;
          }
          
          templateWorldState.FromDoubleArray(vector);// = //(WorldState)worldStateOrig.Clone();
          templateWorldState.Move(worldStateAndObservation.worldState);
          double difference = worldStateAndObservation.measurement.sensor.GetMeasurementDeviation(templateWorldState, experiment.map, worldStateAndObservation.measurement);
          if (!highWeightForNewerMeasurements) weight = 1; //??
          sumOfDistances += Math.Pow(difference, 2) * weight;
        }//if (worldStateAndObservation.measurement != null)
      }// for each entry in history
      return sumOfDistances;
    }
  }
}
